
#ifndef __MLS_DEFORMER_H__
#define __MLS_DEFORMER_H__

#include "mlsutils.h"
#include "deformer.h"

using namespace std;

/// Given two control points sets, this class 
/// performs a deformation using optimal rigid-body 
/// transformations in a MLS-sense
template <class T>
class MlsDeformer: public Deformer {
   
   typedef TConstraints<Point3>     PConstraints;
   typedef TConstraints<Segment3>   SConstraints;
public:
   MlsDeformer() {
      coSource = 0;
      coTarget = 0;
   }
   virtual ~MlsDeformer() { }
   
   void setSourceConstraints(const T &constraints) {
      coSource = &constraints;
   }
   
   void setTargetConstraints(const T &constraints) {
      coTarget = &constraints;
   }
   
   void genDistanceTable(const Point3 &P, const PConstraints *constraints) {
      
      distanceTable.resize(1);
      distanceTable[0].resize(constraints->size(),0.0);
      
      for (uint j=0; j<distanceTable[0].size(); ++j) {
         const Point3 &p = constraints->getConstraint(j);
         Vector3 v(p,P);
         distanceTable[0][j] = sqrt(v*v);
      }
   }
   
   /// Generates the table of weights. Each entry [i][j] maps the weight 
   /// corresponding between the joint i and the constraint j
   void genWeightTable(const Point3 &P, const PConstraints *constraints) {
      
      genDistanceTable(P, constraints);
      weightTable.resize(1);
      weightTable[0].resize(constraints->size(),0.0);
      
      for (uint j=0; j<weightTable[0].size(); ++j) {
         weightTable[0][j] = omega(distance(0, j));
      }
   }
   
   void genDistanceTable(const Point3 &P, const SConstraints *constraints) {
      
      dtTable.resize(1);
      dtTable[0].resize(constraints->size(), make_pair(0.0,0.0));
      
      Number t;
      for (uint j=0; j<dtTable[0].size(); ++j) {
         const Segment3 &segment = constraints->getConstraint(j);
         Number d = computeDistanceMinSegment(P,segment,t);
         dtTable[0][j] = make_pair(d,t);
      }
   }
   
   void genWeightTable(const Point3 &P, const SConstraints *constraints) {
      /// esta misma funcion esta edefinida en el mlsSkelMeshDeformer->verificar
      genDistanceTable(P, constraints);
      weightMatrixTable.resize(1);
      weightMatrixTable[0].resize(constraints->size(), Matrix(2,2));
      
      for (uint j=0; j<weightMatrixTable[0].size(); ++j) {
         Matrix &W = weightMatrixTable[0][j]; /// matrix of weights
         Number d, t;
         distance(0,j,d,t); // return the distance to the bone j. d:distance, t:parameter for define the closest point
         const Number peso = omega(d);
         
         /// constant version
         //~ W(0,0)=peso/3.0;  W(0,1)=peso/6.0;
         //~ W(1,0)=W(0,1);    W(1,1)=W(0,0);
         
         /// parametrized version
         //~ W(0,0)=peso*t*t;       W(0,1)=peso*t*(1.0-t);
         //~ W(1,0)=W(0,1);         W(1,1)=peso*(1.0-t)*(1.0-t);
         
         W(0,0)=0; W(0,1)=0;
         W(1,0)=0; W(1,1)=0;
      }
   }
   
   Number omega(const Number &d) {
      if (d==0) return 1e8;
      return 1.0/(d*d); // 1/|d|^2
   }
   
   const Number& weight(int i, int j) const {
      return weightTable[i][j];
   }
   
   const Number& distance(int i, int j) const {
      return distanceTable[i][j];
   }
   
   void distance(int i, int j, Number &d, Number &t) {
      d = dtTable[i][j].first;
      t = dtTable[i][j].second;
   }
   
   const Matrix& weightMatrix(int i, int j) const {
      return weightMatrixTable[i][j];
   }
   
   Point3 computeCentroid(int id, const PConstraints *constraints) {
      
      Number x=0, y=0, z=0, ws=0;
      for (uint i=0; i<constraints->size(); ++i) {
         const Number &w = weight(id, i);
         const Point3 &p = constraints->getConstraint(i);
         ws += w;
         x += w*p[0];
         y += w*p[1];
         z += w*p[2];
      }
      return Point3(x/ws, y/ws, z/ws);
   }
   
   void computeCorrelationMatrix(int id, const PConstraints *cS, const PConstraints *cT) {
      
      Mf0[0]=Mf0[1]=Mf0[2]=
      Mf1[0]=Mf1[1]=Mf1[2]=
      Mf2[0]=Mf2[1]=Mf2[2]=0.0;
      Number pihat[3], qihat[3];
      for (uint i=0; i<cS->size(); ++i) {
         const Number &w = weight(id, i);
         const Point3 &p = cS->getConstraint(i);
         const Point3 &q = cT->getConstraint(i);
         
         pihat[0] = (p[0]-pstar[0])*w;
         pihat[1] = (p[1]-pstar[1])*w;
         pihat[2] = (p[2]-pstar[2])*w;
         
         qihat[0] = (q[0]-qstar[0]);
         qihat[1] = (q[1]-qstar[1]);
         qihat[2] = (q[2]-qstar[2]);
         
         //~ if (i==0) {
            //~ cout<<":: "<<endl;
            //~ cout<<"   "<<pihat[0]<<" "<<pihat[1]<<" "<<pihat[2]<<endl;
            //~ cout<<"   "<<qihat[0]<<" "<<qihat[1]<<" "<<qihat[2]<<endl;
         //~ }
         
         Mf0[0]+=qihat[0]*pihat[0]; Mf0[1]+=qihat[0]*pihat[1]; Mf0[2]+=qihat[0]*pihat[2];
         Mf1[0]+=qihat[1]*pihat[0]; Mf1[1]+=qihat[1]*pihat[1]; Mf1[2]+=qihat[1]*pihat[2];
         Mf2[0]+=qihat[2]*pihat[0]; Mf2[1]+=qihat[2]*pihat[1]; Mf2[2]+=qihat[2]*pihat[2];
      }
      
      detM = Mf0[0]*(Mf1[1]*Mf2[2]-Mf1[2]*Mf2[1])+
             Mf0[1]*(Mf1[2]*Mf2[0]-Mf1[0]*Mf2[2])+
             Mf0[2]*(Mf1[0]*Mf2[1]-Mf2[0]*Mf1[1]);
      E = Mf0[0] + Mf1[1] + Mf2[2];
      V[0]=Mf1[2]-Mf2[1]; V[1]=Mf2[0]-Mf0[2]; V[2]=Mf0[1]-Mf1[0];
      
      //~ if (id==3) {
         //~ cout<<"<>"<<endl;
         //~ cout<<Mf0[0]<<" "<<Mf0[1]<<" "<<Mf0[2]<<endl;
         //~ cout<<Mf1[0]<<" "<<Mf1[1]<<" "<<Mf1[2]<<endl;
         //~ cout<<Mf2[0]<<" "<<Mf2[1]<<" "<<Mf2[2]<<endl;
         //~ cout<<": "<<detM<<" "<<E<<" : "<<V[0]<<" "<<V[1]<<" "<<V[2]<<endl;
      //~ }
   }
   
   Point3 computeCentroid(int id, const SConstraints *constraints) {
      
      Number x=0, y=0, z=0, ws=0;
      for (uint i=0; i<constraints->size(); ++i) {
         const Matrix &W = weightMatrix(id, i);
         const Segment3 &s = constraints->getConstraint(i);
         const Point3 &a = s[0];
         const Point3 &b = s[1];
         //~ cout<<"id: W"<<W<<endl;
         /// constant version
         //~ Number w = W(0,0)+2.0*W(0,1)+W(1,1);
         //~ ws += w;
         //~ x += w*0.5*(a[0]+b[0]);
         //~ y += w*0.5*(a[1]+b[1]);
         //~ z += w*0.5*(a[2]+b[2]);
         
         /// parametrized version
         //~ Number w = W(0,0)+2.0*W(0,1)+W(1,1);
         //~ ws += w;
         //~ x += (W(0,0)+W(1,0))*b[0] + (W(0,1)+W(1,1))*a[0];
         //~ y += (W(0,0)+W(1,0))*b[1] + (W(0,1)+W(1,1))*a[1];
         //~ z += (W(0,0)+W(1,0))*b[2] + (W(0,1)+W(1,1))*a[2];
         
         Number w = W(0,0)+2.0*W(0,1)+W(1,1);
         ws += w;
         x += (W(0,1)+W(1,1))*b[0] + (W(0,0)+W(0,1))*a[0];
         y += (W(0,1)+W(1,1))*b[1] + (W(0,0)+W(0,1))*a[1];
         z += (W(0,1)+W(1,1))*b[2] + (W(0,0)+W(0,1))*a[2];
      }
      return Point3(x/ws, y/ws, z/ws);
   }
   
   void computeCorrelationMatrix(int id, const SConstraints *cS, const SConstraints *cT) {
      //~ cout<<"computeCorrelationMatrix() ...: "<<cS->size()<<endl;
      Matrix M(3,3), ba(2,3), dc(2,3), ab(2,3), cd(2,3);
      M.zero();
      for (uint i=0; i<cS->size(); ++i) {
         const Matrix &W = weightMatrix(id, i);
         const Segment3 &ss = cS->getConstraint(i);
         const Segment3 &st = cT->getConstraint(i);
         //~ cout<<"W: "<<W<<endl;
         //~ cout<<"ss: "<<ss<<endl;
         //~ cout<<"st: "<<st<<endl;
         const Point3 &a = ss[0];
         const Point3 &b = ss[1];
         const Point3 &c = st[0];
         const Point3 &d = st[1];
         
         const Vector3 &ahat=a-pstar;
         const Vector3 &bhat=b-pstar;
         const Vector3 &chat=c-qstar;
         const Vector3 &dhat=d-qstar;
         
         //~ cout<<"aaa: "<<endl;
         //~ ba(0,0)=bhat.x(); ba(0,1)=bhat.y(); ba(0,2)=bhat.z();
         //~ ba(1,0)=ahat.x(); ba(1,1)=ahat.y(); ba(1,2)=ahat.z();
         //~ dc(0,0)=dhat.x(); dc(0,1)=dhat.y(); dc(0,2)=dhat.z();
         //~ dc(1,0)=chat.x(); dc(1,1)=chat.y(); dc(1,2)=chat.z();
         //~ M = M + t(dc)*W*ba;
         //~ cout<<"ccccccccc: "<<endl;
         
         ab(0,0)=ahat[0]; ab(0,1)=ahat[1]; ab(0,2)=ahat[2];
         ab(0,0)=bhat[0]; ab(0,1)=bhat[1]; ab(0,2)=bhat[2];
         cd(0,0)=chat[0]; cd(0,1)=chat[1]; cd(0,2)=chat[2];
         cd(0,0)=dhat[0]; cd(0,1)=dhat[1]; cd(0,2)=dhat[2];
         M = M + t(cd)*W*ab;
      }
      
      Mf0[0]=M(0,0); Mf0[1]=M(0,1); Mf0[2]=M(0,2);
      Mf1[0]=M(1,0); Mf1[1]=M(1,1); Mf1[2]=M(1,2);
      Mf2[0]=M(2,0); Mf2[1]=M(2,1); Mf2[2]=M(2,2);
      
      detM = Mf0[0]*(Mf1[1]*Mf2[2]-Mf1[2]*Mf2[1])+
             Mf0[1]*(Mf1[2]*Mf2[0]-Mf1[0]*Mf2[2])+
             Mf0[2]*(Mf1[0]*Mf2[1]-Mf2[0]*Mf1[1]);
      
      cout<<id<<" :detM: "<<detM<<" : "<<endl<<M<<endl;
      
      E = Mf0[0] + Mf1[1] + Mf2[2];
      V[0]=Mf1[2]-Mf2[1]; V[1]=Mf2[0]-Mf0[2]; V[2]=Mf0[1]-Mf1[0];
   }
   
   Number computeLargestRootOfQuarticEquation() {
      
      /// Build the cubic equation (economic technique)
      Number normaM2=0.0, prodesc2=0.0, prodNorma2=0.0, normaM[3];
      for (int i=0; i<3; i++) {
         normaM[i]=Mf0[i]*Mf0[i]+Mf1[i]*Mf1[i]+Mf2[i]*Mf2[i];
         normaM2 += normaM[i];
      }
      for (int i=0; i<3; i++) {
         int j=(i+1)%3;
         const Number prod_dot = Mf0[i]*Mf0[j]+Mf1[i]*Mf1[j]+Mf2[i]*Mf2[j];
         prodesc2 += prod_dot*prod_dot;
         prodNorma2 += normaM[i]*normaM[j];
      }
      const Number a = -4.0*normaM2;
      const Number b = 16.0*(prodNorma2-prodesc2);
      const Number c = -64.0*detM*detM;
      
      /// Get one cubic root
      const Number p2 = getCubitRoot_A(a,b,c);
      const Number p = sqrt(p2);
      const Number minusp2_plus_4M2 = -p2 + 4.0*normaM2;
      const Number _16detMdivp = (16.0*detM)/p;
      
      /// the largest root
      Number maxroot = max(-p + sqrt(minusp2_plus_4M2 - _16detMdivp), 
                            p + sqrt(minusp2_plus_4M2 + _16detMdivp));
      
      return maxroot*0.5;
   }
   
   void computeRotationVector() {
      
      /// Find the u value
      Nf0[0]=Mf0[0]+Mf0[0]; Nf0[1]=Mf0[1]+Mf1[0]; Nf0[2]=Mf0[2]+Mf2[0];
      Nf1[0]=Mf1[0]+Mf0[1]; Nf1[1]=Mf1[1]+Mf1[1]; Nf1[2]=Mf1[2]+Mf2[1];
      Nf2[0]=Mf2[0]+Mf0[2]; Nf2[1]=Mf2[1]+Mf1[2]; Nf2[2]=Mf2[2]+Mf2[2];
      
      Nf0[0] -= eigenvalue;
      Nf1[1] -= eigenvalue;
      Nf2[2] -= eigenvalue;
      
      /// Solve the 3x3 system
      const Number sigma = (Nf0[0]*Nf2[1] - Nf2[0]*Nf0[1])/(Nf0[0]*Nf1[1] - Nf1[0]*Nf0[1]);
      u[2] = (Nf0[0]*V[2]  -Nf2[0]*V[0]  -sigma*(Nf0[0]*V[1] - Nf1[0]*V[0]))/
             (Nf0[0]*Nf2[2]-Nf2[0]*Nf0[2]-sigma*(Nf0[0]*Nf1[2]-Nf1[0]*Nf0[2]));
      u[1] = ((Nf0[0]*V[1] -Nf1[0]*V[0])-(Nf0[0]*Nf1[2]-Nf1[0]*Nf0[2])*u[2])/
             (Nf0[0]*Nf1[1] -Nf1[0]*Nf0[1]);
      u[0] = (V[0]-Nf0[2]*u[2]-Nf0[1]*u[1])/Nf0[0];
      
      /// Solve the 3x3 system
      //~ int pl=-1, pc=-1;
      //~ if (Nf0[0] == 0) {
         //~ for (int i=1; i<3; ++i) 
            //~ if (Nf0[i] != 0) { pl = 0; pc = i; }
         //~ if (pc == -1) {
            //~ for (int i=0; i<3; ++i) 
               //~ if (Nf1[i] != 0) { pl = 1; pc = i; }
         //~ }
         //~ if (pc == -1) {
            //~ for (int i=0; i<3; ++i) 
               //~ if (Nf2[i] != 0) { pl = 2; pc = i; }
         //~ }
         
         //~ if (pc == -1) u[0] = u[1] = u[2] = 0; return;
         //~ if (pl != 0) {
            //~ Number tmp[4] = {Nf0[0], Nf0[1], Nf0[2], V[0]};
            //~ if (pl == 1) {
               //~ Nf0[0] = Nf1[0]; Nf0[1] = Nf1[1]; Nf0[2] = Nf1[2]; V[0] = V[1];
               //~ Nf1[0] = tmp[0]; Nf1[1] = tmp[1]; Nf1[2] = tmp[2]; V[1] = tmp[3];
            //~ }
            //~ else {
               //~ Nf0[0] = Nf2[0]; Nf0[1] = Nf2[1]; Nf0[2] = Nf2[2]; V[0] = V[2];
               //~ Nf1[0] = tmp[0]; Nf1[1] = tmp[1]; Nf1[2] = tmp[2]; V[2] = tmp[3];
            //~ }
         //~ }
         //~ if (pc != 0) {
            //~ Number tmp[3] = {Nf0[0], Nf1[0], Nf2[0]};
            //~ if (pc == 1) {
               //~ Nf0[0] = Nf0[1]; Nf1[0] = Nf1[1]; Nf2[0] = Nf2[1];
               //~ Nf0[1] = tmp[0]; Nf1[1] = tmp[1]; Nf2[1] = tmp[2];
            //~ }
            //~ else {
               //~ Nf0[0] = Nf0[2]; Nf1[0] = Nf1[2]; Nf2[0] = Nf2[2];
               //~ Nf0[2] = tmp[0]; Nf1[2] = tmp[1]; Nf2[2] = tmp[2];
            //~ }
         //~ }
      //~ }
      
      //~ if ((Nf0[0]*Nf1[1] - Nf1[0]*Nf0[1]) != 0) {
         //~ const Number sigma = (Nf0[0]*Nf2[1] - Nf2[0]*Nf0[1])/(Nf0[0]*Nf1[1] - Nf1[0]*Nf0[1]);
         //~ const Number den = (Nf0[0]*Nf2[2]-Nf2[0]*Nf0[2]-sigma*(Nf0[0]*Nf1[2]-Nf1[0]*Nf0[2]));
         //~ if (den != 0) u[2] = (Nf0[0]*V[2]  -Nf2[0]*V[0]  -sigma*(Nf0[0]*V[1] - Nf1[0]*V[0]))/den;
         //~ else          u[2] = 0;
            
         //~ u[1] = ((Nf0[0]*V[1] -Nf1[0]*V[0])-(Nf0[0]*Nf1[2]-Nf1[0]*Nf0[2])*u[2])/
                //~ (Nf0[0]*Nf1[1] -Nf1[0]*Nf0[1]);
         //~ u[0] = (V[0]-Nf0[2]*u[2]-Nf0[1]*u[1])/Nf0[0];
      //~ }
      //~ else if ((Nf0[0]*Nf1[2] - Nf1[0]*Nf0[2]) != 0) {
         //~ const Number sigma = (Nf0[0]*Nf2[2] - Nf2[0]*Nf0[2])/(Nf0[0]*Nf1[2] - Nf1[0]*Nf0[2]);
         //~ const Number den = (Nf0[0]*Nf2[1]-Nf2[0]*Nf0[1]-sigma*(Nf0[0]*Nf1[1]-Nf1[0]*Nf0[1]));
         //~ if (den != 0) u[1] = (Nf0[0]*V[1]  -Nf2[0]*V[0]  -sigma*(Nf0[0]*V[2] - Nf1[0]*V[0]))/den;
         //~ else          u[1] = 0;
            
         //~ u[2] = ((Nf0[0]*V[2] -Nf1[0]*V[0])-(Nf0[0]*Nf1[1]-Nf1[0]*Nf0[1])*u[1])/
                //~ (Nf0[0]*Nf1[2] -Nf1[0]*Nf0[2]);
         //~ u[0] = (V[0]-Nf0[1]*u[1]-Nf0[2]*u[2])/Nf0[0];
      //~ }
      //~ else if ((Nf0[0]*Nf2[1] - Nf2[0]*Nf0[1]) != 0) {
         //~ u[2] = 0;
         //~ u[1] = (Nf0[0]*V[1] -Nf2[0]*V[0])/(Nf0[0]*Nf2[1] -Nf2[0]*Nf0[1]);
         //~ u[0] = (V[0]-Nf0[1]*u[1])/Nf0[0];
      //~ }
      //~ else if ((Nf0[0]*Nf2[2] - Nf2[0]*Nf0[2]) != 0) {
         //~ u[1] = 0;
         //~ u[2] = (Nf0[0]*V[2] -Nf2[0]*V[0])/(Nf0[0]*Nf2[2] -Nf2[0]*Nf0[2]);
         //~ u[0] = (V[0]-Nf0[2]*u[2])/Nf0[0];
      //~ }
      //~ else {
         //~ u[1] = 0;
         //~ u[2] = 0;
         //~ u[0] = V[0]/Nf0[0];
      //~ }
      
      //~ if (pc > 0) {
         //~ Number tmp = u[0]; u[0] = u[pc]; u[pc] = tmp;
      //~ }
   }
   
   Vector3 rotate(const Vector3 &v) {
      
      /// Perform the rotation of v
      Number vxu[3], rv[3], factor;
      vxu[0] = v[1]*u[2]-v[2]*u[1];
      vxu[1] = v[2]*u[0]-v[0]*u[2];
      vxu[2] = v[0]*u[1]-v[1]*u[0];
      
      factor = -2.0/(1.0 + u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
      rv[0] = u[1]*vxu[2] - u[2]*vxu[1] + vxu[0];
      rv[1] = u[2]*vxu[0] - u[0]*vxu[2] + vxu[1];
      rv[2] = u[0]*vxu[1] - u[1]*vxu[0] + vxu[2];
      
      rv[0] = v[0] + factor*rv[0];
      rv[1] = v[1] + factor*rv[1];
      rv[2] = v[2] + factor*rv[2];
      
      return Vector3(rv[0], rv[1], rv[2]);
   }
   
   Point3 eval(const Point3 &p) {
      
      // generate the table of weights
      genWeightTable(p, coSource);
      
      pstar = computeCentroid(0, coSource);
      qstar = computeCentroid(0, coTarget);
      
      //~ computeCorrelationMatrix(0, coSource, coTarget);
      //~ Number maxroot = computeLargestRootOfQuarticEquation();
      //~ eigenvalue = maxroot + E;
      //~ computeRotationVector(); // computes the u value
      //~ Vector3 rv = rotate(p-pstar);
      
      Vector3 rv = p-pstar;
      return Point3(qstar[0]+rv[0], qstar[1]+rv[1], qstar[2]+rv[2]);
   }
   
protected:
   const T     *coSource;
   const T     *coTarget;
   
   // auxiliar variables
   Number      Mf0[3], Mf1[3], Mf2[3]; // M = Sum w phat x qhat   correlation matrix
   Number      Nf0[3], Nf1[3], Nf2[3]; // N = M + Mt
   Number      V[3]; // 
   Number      E; // the trace of M
   Number      detM; // determinant of M
   Number      eigenvalue;
   Number      u[3];
   Point3      pstar;
   Point3      qstar;
   
   vector<vector<Number> >          distanceTable;
   vector<vector<Number> >          weightTable;
   
   vector<vector<Matrix> >          weightMatrixTable;
   typedef pair<Number, Number>     dt; // distance value and t parameter
   vector<vector<dt> >              dtTable;
};

#endif
